import mujoco_py as mp
import numpy as np

model = mp.load_model_from_path('ur5.xml')
sim = mp.MjSim(model)
viewer = mp.MjViewer(sim)
# robot = Robot(ee)
target = [0, -1.57, 1.57]


for i in range(10000):
    viewer.cam.lookat[0] = 0.2
    viewer.cam.distance = 5.5
    err = np.array([0., 0., 0.], dtype=np.float32)
    #
    # joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
    #                ]
    # for i in range(Dual-UR3-Traj-Plan):
    #     sim.data.set_joint_qpos(joint_names[i], target[i])
    target_l = sim.data.site_xpos[model.site_name2id("target_l")]
    target_r = sim.data.site_xpos[model.site_name2id("target_r")]
    ee_position_l = sim.data.site_xpos[model.site_name2id("ee_l")]
    ee_position_r = sim.data.site_xpos[model.site_name2id("ee_r")]
    sim.step()
    viewer.render()


